//!!!!! New routine to calculate collective term

#include "p30f4011.h"
#include "definesRmat.h"
#include "defines.h"

	extern int collective ;
	extern int pwManual[MAX_INPUTS+1] ;
	extern int pwTrim[MAX_INPUTS+1] ;


void getCollective( void )
{
	union longww temporary ;
	union longww temp ;
// constants defined in Tech Note, 1/(2*(1+sin(theta))  in Q2.14 format
#define const1 21845 // 120 deg, 0.333 in Q2.14 format 
#define const2 32768 //  90 deg, 0.500 in Q2.14 format 
#define const3 18546 // 140 deg, 0.283 in Q2.14 format
// sin(theta) constants
#define sine1 16384 // 120 deg, sine(30) = 0.5 in Q2.14 format 
#define sine2 0     //  90 deg, sine(0) = 0  in Q2.14 format 
#define sine3 18546 // 140 deg, sine(50) = 0.766 in Q2.14 format


	// calculate collective term depending on swash config
#if (SWASH_TYPE == SWASH1)								// 120 deg swash
		temp.WW = ((long)(pwManual[ELEVATOR_INPUT_CHANNEL] - pwTrim[ELEVATOR_INPUT_CHANNEL])*(long)sine1)<<1 ;
		collective = (- (pwManual[AILERON_INPUT_CHANNEL] - pwTrim[AILERON_INPUT_CHANNEL])
					  + (pwManual[AILERON_SECONDARY_INPUT_CHANNEL] - pwTrim[AILERON_SECONDARY_INPUT_CHANNEL])
					  - 2*temp._.W1);
		temporary.WW = __builtin_mulsu( collective ,const1 ) ;
		collective = temporary._.W1 ;


#elif (SWASH_TYPE == SWASH2)							// 90 deg swash
		temp.WW = ((long)(pwManual[ELEVATOR_INPUT_CHANNEL] - pwTrim[ELEVATOR_INPUT_CHANNEL])*(long)sine2)<<1 ;
		collective = (- (pwManual[AILERON_INPUT_CHANNEL] - pwTrim[AILERON_INPUT_CHANNEL])
					  + (pwManual[AILERON_SECONDARY_INPUT_CHANNEL] - pwTrim[AILERON_SECONDARY_INPUT_CHANNEL])
					  - 2*temp._.W1);
		temporary.WW = __builtin_mulsu( collective ,const2 ) ;
		collective = temporary._.W1 ;

#elif (SWASH_TYPE == SWASH3)							// 140 deg swash
		temp.WW = ((long)(pwManual[ELEVATOR_INPUT_CHANNEL] - pwTrim[ELEVATOR_INPUT_CHANNEL])*(long)sine3)<<1 ;
		collective = (- (pwManual[AILERON_INPUT_CHANNEL] - pwTrim[AILERON_INPUT_CHANNEL])
					  + (pwManual[AILERON_SECONDARY_INPUT_CHANNEL] - pwTrim[AILERON_SECONDARY_INPUT_CHANNEL])
					  - 2*temp._.W1);
		temporary.WW = __builtin_mulsu( collective ,const3 ) ;
		collective = temporary._.W1 ;
#endif					  

}
